#include <aversive.h>
#include <aversive/wait.h>
#include <uart.h>
#include <adc.h>

#include "gp2pack.h"

int main(void)
{
  gp2pack gp2p_right;

  // Turn interruptions ON
  sei();

  // Initialize UART
  uart_init();
  fdevopen(uart0_dev_send,uart0_dev_recv);
 
  // bla
  printf("\n\nRobotter 2008 - 102chute - MECA GP2* TEST \n");
  printf("Compiled "__DATE__" at "__TIME__"\n");
  printf("\nBooting...\n");

  // Initialize ADC
  adc_init();

  // Initialize GP2PACK module
  gp2pack_init(&gp2p_right,MUX_ADC2,MUX_ADC0);
  gp2pack_setAB(&gp2p_right,
                2587.090,-1.017,
                11956.714,-0.986);
  gp2pack_setThreshold(&gp2p_right, 90.0, 300.0);

  // Set up led port
  DDRB = 0xFB;
  PORTB = 0x80;

  printf("\ndone.\n\n");
 

  //gp2pack_calibration(&gp2p_right,printf,uart0_recv);

  int i;
  uint16_t d_lr,d_sr;
  uint16_t n=0;
  while(1)
  {
    wait_ms(10);
    gp2pack_update(&gp2p_right);

    // twiliting
    PORTB >>= 1;
    if(!PORTB) PORTB=0x80;
  }

  while(1) nop();

  return 0;
}
